#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class DefaultRobot : public SimpleRobot
{
RobotDrive *myRobot; // robot drive system
Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
Joystick *leftStick; // joystick 2 (tank left stick)
DriverStation *ds; // driver station object
Compressor *KarlKompressor;     // this is a compressor
Solenoid *leftSol; // this is the left solenoid
Solenoid *rightSol; // this is the right solenoid

enum // Driver Station jumpers to control program operation
{ ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
 ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
} jumpers;                            

public:

/**
* 
* 
* Constructor for this robot subclass.
* Create an instance of a RobotDrive with left and right motors plugged into PWM
* ports 1 and 2 on the first digital module.
*/
DefaultRobot(void)
{
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(1, 3, 2, 4); // create robot drive base
rightStick = new Joystick(1); // create the joysticks
leftStick = new Joystick(2);
KarlKompressor = new Compressor(4, 1, 4, 1); 
leftSol = new Solenoid(8,2);
rightSol = new Solenoid(8,3);
//Update the motors at least every 100ms.
GetWatchdog().SetExpiration(0.1);
}

/**
* Drive left & right motors for 2 seconds, enabled by a jumper (jumper
* must be in for autonomous to operate).
*/
void Autonomous(void)
{
GetWatchdog().SetEnabled(false);
}
/**
* Runs the motors under driver control with either tank or arcade steering selected
* by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
*/
void OperatorControl(void)
{

KarlKompressor->Start();
int CylinderPos=0;
myRobot->TankDrive(leftStick, rightStick);
rightSol->Set(CylinderPos);
leftSol->Set(1-CylinderPos);
while (IsOperatorControl())
{
if (leftStick->GetTrigger())
{
CylinderPos = 1 - CylinderPos;
rightSol->Set(1);
leftSol->Set(1-CylinderPos);
}
}
}
};

START_ROBOT_CLASS(DefaultRobot);

